function [paramsValues,Q] = newtonOptim(paramsValues,setupFilter,maxIter,tolFun,tolX)
%This function minimizes an objective function by the classical newton
%method, i.e. using provided scores and hessian matrix, using simple linear
%seach to find the best stepsize

%% Options
display         = 1;
lineSearchDelta = 1;            % 1 for gradual downscaling from 1
                                % 2 for a grid search based on deltaGrid.
deltaGrid       = [1D-4 1D-3 1D-2 0.1 0.25 0.5 0.8 1.0 1.5 2.0];
lambdaStart     = 1D-4;
%% The optimization is done in the transformed space for params
if isfield(setupFilter,'transformParamsOn') == 1
    paramsValues = transformParams(paramsValues,setupFilter.lowerBoundsValues,setupFilter.upperBoundsValues);
end

% Initial value 
%numParams = length(paramsValues);
[Q,errorMes,~,~,outFilter] = objectFuncQML(paramsValues,setupFilter);
if errorMes ~= 0
    disp('Initial point is not defined')
    return
end
diffQ              = 1D5;
idxIter            = 1;
dParamsMax         = 1;
idxNoProgress      = 0;
maxNoProgress      = 5;
updateScoreHessian = 1;
lambda             = lambdaStart;
while abs(diffQ) > tolFun && dParamsMax > tolX && idxIter <= maxIter && idxNoProgress <= maxNoProgress   
    % The score and the hessian
    stepOption = 1;
    if updateScoreHessian == 1
        [hessian,scoreQML,scoreSMM,dQ,dQMLpart,dSMMpart,errorMes] = getScoreHessian(paramsValues,setupFilter,outFilter,stepOption);
        if errorMes ~= 0
            disp('Newton optimizer stopped because score and Hessian cannot be computed')
            break
        end
        % for minimization
        score = -(dQMLpart+dSMMpart);
        hessian = -hessian;
    end
    
    % Computing the new value of params. x_n+1 = x_n + delta, where delta = H^-1*(-score).
    delta  = (hessian + lambda*eye(length(hessian)))\(-score); %or inv(hessian)*score
    
    % Evaluating fit at new value of theta1
    paramsValues_new = paramsValues + delta;
    idxSearch        = 0;
    if any(isnan(paramsValues_new)) || any(isinf(paramsValues_new)) 
        Q_new = Q;
        updateScoreHessian = 0;
    else
        if lineSearchDelta == 1
            [Q_new,errorMes,~,~,outFilter_new] = objectFuncQML(paramsValues_new,setupFilter);
            diffTempQ = Q_new - Q;
            if diffTempQ > 0 || isnan(diffTempQ) 
                % We decrease the scaling to find a better point
                while (diffTempQ > 0 || isnan(diffTempQ)) && idxSearch < 10
                    idxSearch = idxSearch + 1;
                    deltaScaling = 0.8^idxSearch;
                    paramsValues_new = paramsValues + deltaScaling*delta;
                    [Q_new,errorMes,~,~,outFilter_new] = objectFuncQML(paramsValues_new,setupFilter);
                    diffTempQ = Q_new - Q;
                end
            else
                % We increase the scaling to boost the stepsize
                Q0 = Q_new;
                deltaScaling = 1;
                while diffTempQ < 0 && idxSearch < 10
                    idxSearch = idxSearch + 1;
                    deltaScalingTmp = 1.2^idxSearch;
                    paramsValues_newTmp = paramsValues + deltaScalingTmp*delta;
                    [Q_newTmp,errorMes,~,~,outFilter_newTmp] = objectFuncQML(paramsValues_newTmp,setupFilter);
                    diffTempQ    = Q_newTmp - Q0;
                    if diffTempQ < 0
                        deltaScaling     = deltaScalingTmp;
                        paramsValues_new = paramsValues_newTmp;
                        Q_new            = Q_newTmp;
                        outFilter_new    = outFilter_newTmp;
                        Q0               = Q_new;
                    end
                end
            end
        elseif lineSearchDelta == 2
            QGrid_new         = zeros(1,length(deltaGrid));
            outFilterGrid_new = repmat(outFilter,1,length(deltaGrid));
            %parfor i=1:length(deltaGrid) % for multiprocessing
            for i=1:length(deltaGrid)   %for no multiprocessing
                paramsValues_new = paramsValues + deltaGrid(1,i)*delta;
                [QGrid_new(1,i),errorMesGrid,~,~,outFilterGrid_tmp] = objectFuncQML(paramsValues_new,setupFilter);                
                if errorMesGrid == 0
                    outFilterGrid_new(i) = outFilterGrid_tmp;
                end
            end
            [~,idxOpt]       = min(QGrid_new);
            deltaScaling     = deltaGrid(1,idxOpt);
            paramsValues_new = paramsValues + deltaScaling*delta;
            Q_new            = QGrid_new(1,idxOpt);
            outFilter_new    = outFilterGrid_new(idxOpt);
        end
        updateScoreHessian = 1;
    end
    if isfield(setupFilter,'transformParamsOn') == 1
        paramsValues_newUntransform = unTransformParams(paramsValues_new,setupFilter.lowerBoundsValues,setupFilter.upperBoundsValues);
        paramsValuesUntransform     = unTransformParams(paramsValues,setupFilter.lowerBoundsValues,setupFilter.upperBoundsValues);
        [dParamsMaxDisp,paramsPos]  = max(abs(paramsValues_newUntransform- paramsValuesUntransform));
    else
        [dParamsMax,paramsPos] = max(abs(paramsValues_new  - paramsValues));
        dParamsMaxDisp         = dParamsMax;
    end
    if Q_new < Q
        if display == 1
            disp('Best value so far in Newton optimizer:')
            paramsValuesDisp = unTransformParams(paramsValues_new,setupFilter.lowerBoundsValues,setupFilter.upperBoundsValues);
            printToScreen(paramsValuesDisp)
            disp(['dQ         = ', num2str(Q_new - Q)]);
            disp(['Q          = ', num2str(Q_new)]);
            disp(['dParamsMax = ', num2str(dParamsMaxDisp),' (at element ',num2str(paramsPos),')']);
            disp(['deltaScale = ', num2str(deltaScaling)]);
            disp(['lambda     = ', num2str(lambda)]);
            disp(['iteration  = ', num2str(idxIter)]);
            disp(' ');
        end
        paramsValues  = paramsValues_new;
        outFilter     = outFilter_new;
        Q             = Q_new;
        idxNoProgress = 0;
        if deltaScaling >= 0.1
            lambda = lambda/2;
        else
            lambda = lambda*2;
        end
    else
        lambda = lambda*10;
        if display == 1
            disp(['No improvement: Increasing lambda; lambda = ', num2str(lambda)]);
        end
        idxNoProgress = idxNoProgress + 1;
        updateScoreHessian = 0;
    end

    if dParamsMax < 1e-12
        if display == 1
            disp('Change in params is less than 1e-12, so we stop the optimization');
        end
        diffQ = 0;
    end
        
    % Update index
    idxIter  = idxIter + 1;
    if lambda > 10
        if display == 1
            disp('Resetting lambda');
        end
        lambda = lambdaStart;
    end
end
if display == 1
    disp('END OF NEWTON OPTIMIZER')
end
if isfield(setupFilter,'transformParamsOn') == 1
    paramsValues = unTransformParams(paramsValues,setupFilter.lowerBoundsValues,setupFilter.upperBoundsValues);
end
printToScreen(paramsValues)

end


